#ifndef GEOMETRY_UTILS_MATRIXNXN_H
#define GEOMETRY_UTILS_MATRIXNXN_H

#include "GeometryUtilsMath.h"
#include "MatrixNxMBase.h"
#include "VectorNBase.h"
#include <ostream>

namespace geometry_utils
{
template <typename T, size_t N>
struct MatrixNxNBase : MatrixNxMBase<T, N, N>
{
  MatrixNxNBase() : MatrixNxMBase<T, N, N>()
  {
  }
  MatrixNxNBase(T val) : MatrixNxMBase<T, N, N>(val)
  {
  }
  MatrixNxNBase(const MatrixNxNBase& in) : MatrixNxMBase<T, N, N>(in.data)
  {
  }
  MatrixNxNBase(const boost::array<T, N * N>& in) : MatrixNxMBase<T, N, N>(in)
  {
  }
  MatrixNxNBase(T (&in)[N * N]) : MatrixNxMBase<T, N, N>(in)
  {
  }
  MatrixNxNBase(const Eigen::Matrix<T, N, N>& in) : MatrixNxMBase<T, N, N>(in)
  {
  }
  MatrixNxNBase(const MatrixNxMBase<T, N, N>& in) : MatrixNxMBase<T, N, N>(in)
  {
  }

  inline void Eye()
  {
    this->data.fill(0);
    for (size_t i = 0; i < this->nrows; i++)
      this->data[this->nrows * i + i] = 1;
  }

  virtual inline T Det() const
  {
    std::cerr << "MatrixNxMBase::det not implemented" << std::endl;
    return T();
  }

  virtual inline MatrixNxNBase<T, N> Inv() const
  {
    std::cerr << "MatrixNxMBase::inv not implemented" << std::endl;
    return MatrixNxNBase<T, N>();
  }

  static inline MatrixNxNBase<T, N> Diag(const VectorNBase<T, N>& in)
  {
    T d[N * N] = { 0 };
    for (size_t i = 0; i < N; i++)
      d[N * i + i] = in(i);
    return MatrixNxNBase<T, N>(d);
  }
};

template <size_t N>
inline MatrixNxNBase<float, N> operator*(const float& lhs, const MatrixNxNBase<float, N>& rhs)
{
  return MatrixNxNBase<float, N>(rhs * lhs);
}

template <size_t N>
inline MatrixNxNBase<double, N> operator*(const double& lhs, const MatrixNxNBase<double, N>& rhs)
{
  return MatrixNxNBase<double, N>(rhs * lhs);
}

template <typename T, size_t N>
inline MatrixNxNBase<T, N> Inv(const MatrixNxNBase<T, N>& m)
{
  return m.Inv();
}

}  // namespace geometry_utils

#endif
